#include "map_division/OccupancyGridParam.h"
#include "map_division/map_division.h"
#include <fstream>
#include <iostream>
#include <nlohmann/json.hpp>
#include <opencv2/opencv.hpp>
#include <pwd.h>
#include <ros/ros.h>
#include <sstream>
#include <string>

using namespace std;
using json = nlohmann::json;

class MapDivision {
  ros::NodeHandle nh;
  string map_file_path;
  string selected_map;
  string outlinePath;
  string inlinePath;
  string mapPath;
  string zonePath;

  cv::Mat oriMap;
  cv::Mat inborderMap;
  cv::Mat outborderMap;
  cv::Mat edgeMap;

  OccupancyGridParam OccGridParam;
  json layer_data;
  int zone_count;

  vector<cv::Point2d> outlineborder;
  vector<cv::Point2d> inlineborder;
  vector<vector<cv::Point2d>> layerVec;

  ros::Subscriber subMap;
  ros::ServiceServer srvMapdivision;

public:
  MapDivision() {
    nh.param<std::string>("map_file_path", map_file_path,
                          std::string("/.robot/data/maps/"));

    bool if_selected_map_set = ros::param::get("selected_map", selected_map);
    if (if_selected_map_set)
      ROS_INFO_STREAM("Get selected_map = " << selected_map);
    else
      ROS_WARN("Didn't receive selected_map");

    outlinePath = "/home/" + getUserName() + map_file_path + selected_map +
                  "/path/" + "out_border.csv";
    inlinePath = "/home/" + getUserName() + map_file_path + selected_map +
                 "/path/" + "in_border.csv";
    mapPath = "/home/" + getUserName() + map_file_path + selected_map + "/" +
              selected_map + ".png";
    zonePath = "/home/" + getUserName() + map_file_path + selected_map +
               "/map/zone_0/";
    subMap = nh.subscribe("/map", 10, &MapDivision::mapCallback, this);

    srvMapdivision =
        nh.advertiseService("/map/division", &MapDivision::divisionReq, this);
    readPath(outlineborder, outlinePath);
    readPath(inlineborder, inlinePath);
    oriMap = cv::imread(mapPath);
    zone_count = 0;
  }

  void readjson() {
    string json_file_path = "/home/" + getUserName() + map_file_path +
                            selected_map + "/map/layers.json";
    cout << json_file_path << endl;
    fstream f(json_file_path);
    layer_data = json::parse(f);
    cout << layer_data.size() << endl;
    for (json::iterator it = layer_data.begin(); it != layer_data.end(); ++it) {
      if (it.key() == "areas") {
        for (int i = 0; i < it.value().size(); i++) {
          cout << it.value()[i].at("data") << endl;
          auto point_vec = it.value()[i].at("data");
          vector<Point2d> tmp_vec;
          for (int num = 0; num < point_vec.size(); num += 2) {
            Point2d p1(point_vec[num], point_vec[num + 1]);
            tmp_vec.push_back(p1);
          }
          this->layerVec.push_back(tmp_vec);
        }
      }
      if (it.key() == "line") {
        for (int i = 0; i < it.value().size(); i++) {
          cout << it.value()[i].at("data") << endl;
          auto point_vec = it.value()[i].at("data");
          vector<Point2d> tmp_vec;
          for (int num = 0; num < point_vec.size(); num += 2) {
            Point2d p1(point_vec[num], point_vec[num + 1]);
            tmp_vec.push_back(p1);
          }
          this->layerVec.push_back(tmp_vec);
        }
      }
    }
  }

  std::string getUserName() {
    struct passwd *password = getpwuid(getuid());
    if (password)
      return password->pw_name;
    else
      return "UNKNOWN";
  }

  bool readPath(vector<cv::Point2d> &vectorIn, string &infilePath) {
    ifstream inFile(infilePath, ios::in);
    if (!inFile) {
      cout << "Failed to open file" << endl;
      return false;
    }
    string line;
    string field;
    while (getline(inFile, line)) {
      string field;
      istringstream sin(line);
      cv::Point2d point;
      getline(sin, field, ' ');
      point.x = atof(field.c_str());
      getline(sin, field, ' ');
      point.y = atof(field.c_str());
      vectorIn.push_back(point);
    }
    return true;
  }

  void mapCallback(const nav_msgs::OccupancyGrid &msg) {
    OccGridParam.GetOccupancyGridParam(msg);
    cv::Mat m(oriMap.rows, oriMap.cols, CV_8UC1, Scalar(255));
    // cv::Mat n(oriMap.rows, oriMap.cols, CV_8UC3, Scalar(255));
    edgeMap = m.clone();
    inborderMap = m.clone();
    outborderMap = m.clone();

    // fillborder();
    // genEdgeMap();
    // visualization();
  }

  bool divisionReq(map_division::map_divisionRequest &req,
                   map_division::map_divisionResponse &resp) {
    releaseMemory();
    // read json file
    readjson();
    fillborder();
    genEdgeMap();
    visualization();
    if (zone_count == 0) {
      resp.message = "no zones have been divided";
    } else {
      resp.message = "totally divide " + to_string(zone_count - 1) + " zones";
    }
  }

  void releaseMemory()
  {
    zone_count = 0;
    outlineborder.clear();
    inlineborder.clear();
    layerVec.clear();
  }

  void borderPaint(vector<cv::Point2d> &vecIn) {
    // edgeMap = cv::Mat::zeros(OccGridParam.height,OccGridParam.width,
    // CV_8UC1);

    for (int i = 0; i < vecIn.size() - 1; i++) {
      // convert to mat frame
      Point p1;
      OccGridParam.Map2ImageTransform(vecIn[i], p1);
      Point p2;
      OccGridParam.Map2ImageTransform(vecIn[i + 1], p2);
      drawLine(edgeMap, p1, p2);
    }
    int i = vecIn.size() - 1;
    if (i < 2)
      return;
    Point p1;
    OccGridParam.Map2ImageTransform(vecIn[i], p1);
    Point p2;
    OccGridParam.Map2ImageTransform(vecIn[0], p2);
    drawLine(edgeMap, p1, p2);
  }

  void layerPaint(vector<cv::Point2d> &vecIn) {
    for (int i = 0; i < vecIn.size() - 1; i++) {
      // convert to mat frame
      drawLine(edgeMap, vecIn[i], vecIn[i + 1]);
    }
    int i = vecIn.size() - 1;
    if (i < 2)
      return;
    drawLine(edgeMap, vecIn[i], vecIn[0]);
  }

  void borderPaint(cv::Mat img, vector<cv::Point2d> &vecIn) {
    // edgeMap = cv::Mat::zeros(OccGridParam.height,OccGridParam.width,
    // CV_8UC1);

    for (int i = 0; i < vecIn.size() - 1; i++) {
      // convert to mat frame
      Point p1;
      OccGridParam.Map2ImageTransform(vecIn[i], p1);
      Point p2;
      OccGridParam.Map2ImageTransform(vecIn[i + 1], p2);
      drawLine(img, p1, p2);
    }
    int i = vecIn.size() - 1;
    if (i < 2)
      return;
    Point p1;
    OccGridParam.Map2ImageTransform(vecIn[i], p1);
    Point p2;
    OccGridParam.Map2ImageTransform(vecIn[0], p2);
    drawLine(img, p1, p2);
  }

  void drawLine(Mat img, Point start, Point end) {
    int thickness = 8;
    int lineType = 8;
    line(img, start, end, Scalar(0), thickness, lineType);
  }

  void fillborder() {
    borderPaint(inborderMap, inlineborder);
    Rect rec;
    cv::floodFill(inborderMap, Point(0, 0), Scalar(0), &rec, Scalar(20),
                  Scalar(0));
    borderPaint(outborderMap, outlineborder);
    Rect rec_;
    cv::floodFill(outborderMap, Point(0, 0), Scalar(0), &rec_, Scalar(20),
                  Scalar(0));
  }

  void genEdgeMap() {
    for (int i = 0; i < edgeMap.rows; i++) {
      for (int j = 0; j < edgeMap.cols; j++) {
        if (inborderMap.at<uint8_t>(i, j) < 10 &&
            outborderMap.at<uint8_t>(i, j) > 250) {
          edgeMap.at<uint8_t>(i, j) = 255;
        } else {
          edgeMap.at<uint8_t>(i, j) = 0;
        }
      }
    }

    // save zone_0.json
    saveZoneMap(zone_count, edgeMap);

    // 划线
    // vector<cv::Point2d> line;
    // line.push_back(outlineborder[0]);
    // line.push_back(outlineborder[30]);
    // borderPaint(line);
    // cout << layerVec.size() << endl;
    for (auto elm : this->layerVec) {
      for (auto e : elm) {
      }
      layerPaint(elm);
    }

    cv::imshow("edgeMap", edgeMap);
    cv::waitKey(1000);
    cv::Mat maskImage;
    maskImage.create(edgeMap.rows + 2, edgeMap.cols + 2, CV_8UC1);
    maskImage = Scalar::all(255);
    for (int m = 0; m < edgeMap.rows; m++) {
      for (int n = 0; n < edgeMap.cols; n++) {
        maskImage.at<uint8_t>(m + 1, n + 1) = 255 - edgeMap.at<uint8_t>(m, n);
      }
    }

    // 调用分区漫水算法
    // int value;
    // vector<int> color = {20, 40, 60, 80};
    // int color_idx = 0;
    // for (int i = 0; i < edgeMap.rows - 10; i += 10) {
    //   for (int j = 0; j < edgeMap.cols - 10; j += 10) {
    //     if (edgeMap.at<uint8_t>(i, j) > 100 &&
    //         edgeMap.at<uint8_t>(i, j + 10) > 100 &&
    //         edgeMap.at<uint8_t>(i, j - 10) > 100 &&
    //         edgeMap.at<uint8_t>(i + 10, j) > 100 &&
    //         edgeMap.at<uint8_t>(i - 10, j) > 100) {
    //       // cout << "a new zone" << endl;
    //       Rect rec;
    //       cv::Mat tmp_zone;
    //       value = cv::floodFill(edgeMap, maskImage, Point(j, i),
    //                             Scalar(color[color_idx]), &rec, Scalar(10),
    //                             Scalar(0), FLOODFILL_FIXED_RANGE);
    //       // cout << value << endl;
    //       if (!value)
    //         continue;

    //       tmp_zone = edgeMap.clone();
    //       for (int m = 0; m < edgeMap.rows; m++) {
    //         for (int n = 0; n < edgeMap.cols; n++) {
    //           if (tmp_zone.at<uint8_t>(m, n) == color[color_idx]) {
    //             tmp_zone.at<uint8_t>(m, n) = 255;
    //           } else {
    //             tmp_zone.at<uint8_t>(m, n) = 0;
    //           }
    //         }
    //       }
    //       cv::imshow("tmp_zone", tmp_zone);
    //       cv::imshow("edgeMap", edgeMap);
    //       cv::waitKey(1000);
    //       color_idx += 1;
    //       saveZoneMap(zone_count, tmp_zone);
    //       // cv::imshow("maskMap", maskImage);
    //       // cv::waitKey(0);
    //     }
    //   }
    // }

    //调用种子点生长算法
    for (int m = 0; m < edgeMap.rows; m++) {
      for (int n = 0; n < edgeMap.cols; n++) {
        cv::Point tmp(n, m);
        regionGrow(edgeMap, tmp, 10);
      }
    }
  }

  bool regionGrow(cv::Mat img, cv::Point seed, int threshold) {
    if (seed.x < 0 || seed.y < 0 || seed.y > img.rows - 1 ||
        seed.x > img.cols - 1) {
      return false;
    }

    if (img.ptr<uchar>(seed.y)[seed.x] == 0) {
      return false;
    }
    cv::Mat tmp_zone;
    tmp_zone.create(img.rows, img.cols, CV_8UC1);
    tmp_zone = Scalar(0);
    vector<Point2i> seeds;
    vector<Point2i> seeds_cache;
    seeds.push_back(seed);
    seeds_cache.push_back(seed);
    int growDirections[8][2] = {{-1, -1}, {0, -1}, {1, -1}, {-1, 0},
                                {1, 0},   {-1, 1}, {0, 1},  {1, 1}}; //生长方向
    while (!seeds.empty()) {
      Point2i seed_current = seeds.back();
      seeds.pop_back();
      // img.ptr<uchar>(seed_current.y)[seed_current.x] = 0;
      for (int i = 0; i < 8; i++) {
        Point2i neighborPoint = {seed_current.x + growDirections[i][0],
                                 seed_current.y + growDirections[i][1]}; //邻点
        if (neighborPoint.x < 0 || neighborPoint.y < 0 ||
            neighborPoint.x > img.cols - 1 ||
            neighborPoint.y > img.rows - 1) { //邻点超出范围
          continue;
        }
        if ((tmp_zone.ptr<uchar>(neighborPoint.y)[neighborPoint.x] == 0) &&
            abs(img.ptr<uchar>(neighborPoint.y)[neighborPoint.x] -
                img.ptr<uchar>(seed.y)[seed.x]) < threshold) {
          tmp_zone.ptr<uchar>(neighborPoint.y)[neighborPoint.x] = 255;
          seeds.push_back(neighborPoint);
          seeds_cache.push_back(neighborPoint);
        }
      }
    }
    // paint the edge_map
    for (int i = 0; i < seeds_cache.size(); i++) {
      edgeMap.ptr<uchar>(seeds_cache[i].y)[seeds_cache[i].x] = 0;
    }
    saveZoneMap(zone_count, tmp_zone);
    cv::imshow("tmp_zone", tmp_zone);
    cv::imshow("edge_map", edgeMap);
    cv::waitKey(1000);
  }

  void saveZoneMap(int id, cv::Mat img) {
    string mat_name = zonePath + "zone_" + to_string(id) + ".png";
    cout << mat_name << endl;
    cv::imwrite(mat_name, img);
    zone_count++;
  }

  void visualization() {
    cv::namedWindow("oriMap");
    cv::imshow("oriMap", oriMap);
    cv::namedWindow("edgeMap");
    cv::imshow("edgeMap", edgeMap);
    cv::namedWindow("inborderMap");
    cv::imshow("inborderMap", inborderMap);
    cv::namedWindow("outborderMap");
    cv::imshow("outborderMap", outborderMap);
    cv::waitKey(1000);
    cv::destroyAllWindows();
    return;
  }
};

int main(int argc, char **argv) {
  cout << "jt-map-division" << endl;
  ros::init(argc, argv, "waypath_viable");
  MapDivision mp;
  ros::spin();
  return 0;
}
